/*
 * FineLocalization.c
 *
 *  Created on: Nov 29, 2012
 *      Author: pmelick
 */

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#define RADIUS 121.9 //Distance of the corner of the car from the center of the secondary coil
#define TAGANGLE 30.0*3.1416/180.0//Angle of the corner of the car from the center of the secondary coil as measured from the forward direction of the car
#define PI 3.1416
#define XURF 20.2//X distance from the starting point of the primary coil to the URF
#define YURF 152.2//Y distance from the starting point of the primary coil to the URF
#define L 97

static float xCar;
static float yCar;
static float phiCar;

/* The coarse localization function for the URFs.
 * r1, r2, theta1, and theta2 are the distances and angles the corners of the car as measured
 * in a coordinate system whose origin is at the URF, whose x axis is the right and y axis is forwards
 * when facing the car, from the front of the parking space.  The module variables xCar yCar and phiCar 
 * are then the coarse solution for the location of the car.  They are with respect to a coordinate system 
 * whose origin is the starting point of the primary coil, and whose x axis is to the right and whose 
 * y axis is forwards when facing the forward direction of the car.
 */

void coarseLocalize( float r1, float r2, float theta1, float theta2)
{
	theta1=theta1*PI/180.; 
	theta2=theta2*PI/180.;

	float x1;
	float x2;
	float y1;
	float y2;
	float a;
	float b;
	
	x1=XURF-r1*cos(theta1);
	x2=XURF-r2*cos(theta2);
	y1=YURF-r1*sin(theta1);
	y2=YURF-r2*sin(theta2);
	
	phiCar=atan((y1-y2)/(x2-x1));
	a=(x1+x2)/2;
	b=(y1+y2)/2;
	xCar=a+L*sin(phiCar);
	yCar=b-L*cos(phiCar);
	//a=RADIUS*sin(phiCar+TAGANGLE);
	//b=RADIUS*cos(phiCar+TAGANGLE);
	//xCar=(x1+x2+b-a)/2;
	//yCar=(y1+y2-a-b)/2;
}

float getXCarCoarse(void)
{
	return xCar;
}

float getYCarCoarse(void)
{
	return yCar;
}

float getPhiCarCoarse(void)
{
	return phiCar;
}
